#include "OOPSMP/Graphics/GUtils/GGeometry/GWorkspace/GArticulatedPart3D.H"
#include "OOPSMP/Utils/Math/Algebra/matrix44.h"

/**
 *@brief Auxiliary function for drawing 3D part geometry 
 *@param gRenderer graphical renderer
 *@param part workspace articulated part
 */ 
void OOPSMP::GArticulatedPart3D::drawPartGeometry(OOPSMP::GRenderer_t  gRenderer, 
				       OOPSMP::Part_const_t part)
{
    ArticulatedPart3D_t  articulatedPart3D    = const_cast<ArticulatedPart3D_t>(dynamic_cast<ArticulatedPart3D_const_t>(part));
	
	if ( articulatedPart3D == NULL )
		drawPartGeometryHelper(gRenderer, part);
	else {
		int n = articulatedPart3D->getLinkCount();

		for (int k = 0; k < n; k++) {

			// we want to draw joints for all links beyond the tail (because joints are attached in front of the link)
			if( k > 0 ) {
				// we need to find the center of the joint we want to paint
				double * jCenter = new double[3];
				// double * jCenter = (double *)calloc( 3, sizeof(double) );

				// get link's joint attachment point (untransformed)
				articulatedPart3D->getJointCenterPointForLink( k, jCenter );
				
				/*printf( "Entering getJointCenterPointForLink( k = %d ):\n", k );
				printf( "JointCenterPoint for link %d: [%f %f %f]\n", k, jCenter[0], jCenter[1], jCenter[2] );*/

				// get link's transformation and transform jCenter
				OOPSMPmatrix44_t m = (OOPSMPmatrix44_t) articulatedPart3D->getLinkPart(k)->getTransformation();
				OOPSMPtransform_point3_matrix44(jCenter, m, jCenter);
				
				// draw the sphere at transformed jCenter, with r = 0.45 * jointLength
				gRenderer->renderSphere3D(jCenter[0], jCenter[1], jCenter[2], articulatedPart3D->getJointLengthForLink(k) * 0.45);
				delete [] jCenter;
				// free( jCenter );
			}
			drawPartGeometryHelper(gRenderer, articulatedPart3D->getLinkPart(k));
		}
	}
}

/**
 *@brief Draw selected part from articulated part
 *@param gRenderer graphical renderer
 *@param part workspace part
 */ 
void OOPSMP::GArticulatedPart3D::drawPartGeometryHelper(OOPSMP::GRenderer_t  gRenderer, 
				       OOPSMP::Part_const_t part)
{
	Part3D_t                    part3D    = const_cast<Part3D_t>(dynamic_cast<Part3D_const_t>(part)); 
	const int                   nrTri     = part3D->getNrTriangles();
	OOPSMPmatrix44_t            m         = (OOPSMPmatrix44_t) part3D->getTransformation();
	OOPSMPdeclare_vector3(aNormal);
	OOPSMPdeclare_vector3(bNormal);
	OOPSMPdeclare_vector3(cNormal);    
	OOPSMPdeclare_matrix44(mNormals);
	double                      mtri[9];

	if(part3D == NULL || gRenderer == NULL)
		return;

	gRenderer->renderMaterial(part->getMaterial());

	if(part->getMaterial()->isUsingPointModel())
	{
		double p[3];

		part->getSomePoint(p);
		//gRenderer->renderPoint3D(p[0], p[1], p[2]);
		gRenderer->renderSphere3D(p[0], p[1], p[2], 0.01);
	}
	else
	{
		OOPSMPidentity_matrix44(mNormals);
		OOPSMPinverse_matrix44(m, mNormals);
		OOPSMPinplace_transpose_matrix44(mNormals);		
		for(int i = 0; i < nrTri; i++)
		{
			const int *ti = part3D->getTriangle(i);

			OOPSMPtransform_vector3_matrix44(part3D->getNormal(ti[0]), mNormals, aNormal);
			OOPSMPtransform_vector3_matrix44(part3D->getNormal(ti[1]), mNormals, bNormal);
			OOPSMPtransform_vector3_matrix44(part3D->getNormal(ti[2]), mNormals, cNormal);

			for(int j = 0; j < 3; j++)
				OOPSMPtransform_point3_matrix44(part3D->getVertex(ti[j]), m, &(mtri[3 * j]));
			gRenderer->renderTriangle3D(mtri, aNormal, bNormal, cNormal);
		}
	}
}